using System;
namespace Blimpbots.Control {
    class OOBTwoDimBlimpController {
        private String name;

        private void report(String s) {
            Console.WriteLine("oob2DC_" + name + ": " + s);
            return;
        }

        public OOBTwoDimBlimpController(String _name) {
            name = _name;
        }

        public void update(double currentOrientation, double desiredOrientation, double urgency,
            out double leftMotorSetting, out double rightMotorSetting) {

            double error = normalizeOrientation(desiredOrientation - currentOrientation);

            report("error: " + error);

            double minMotorSpeed = 3;
            double maxMotorSpeed = 7;

            if (urgency > 1) {
                report("ERROR: bad urgency");
                urgency = 1;
            }
            if (urgency < 0) {
                report("ERROR: bad urgency");
                urgency = 0;
            }

            double max = minMotorSpeed + urgency*(maxMotorSpeed - minMotorSpeed);

            // error  dir   L    R
            // -pi    back  -1   -1
            // -3pi/4 backR 0    -1
            // -pi/2  right 1    -1
            // -pi/4  fwdR  1    0
            // 0      fwd   1    1
            // pi/4   fwdL  0    1
            // pi/2   left  -1   1
            // 3pi/4  backL -1   0
            // pi     bacl  -1   -1

            double negPiOver2 = -1*(Math.PI/2.0);
            double threePiOver4 = (3*Math.PI)/4.0; 
            double PiOver4 = Math.PI/4.0;
            double PiOver2 = Math.PI/2.0;

            if (error < negPiOver2) {
                rightMotorSetting = -1 * max;
                leftMotorSetting = max * ((error + threePiOver4) / PiOver4);
            }
            else if (error < 0) {
                rightMotorSetting = max * ((error + PiOver4) / PiOver4);
                leftMotorSetting = max;
            }
            else if (error < PiOver2) {
                rightMotorSetting = max;
                leftMotorSetting = -1 * max * ((error - PiOver4) / PiOver4);
            }
            else {
                rightMotorSetting = -1 * max * ((error - threePiOver4) / PiOver4);
                leftMotorSetting = -1 * max;
            }

            if (leftMotorSetting > maxMotorSpeed || rightMotorSetting > maxMotorSpeed ||
                -1 * leftMotorSetting > maxMotorSpeed || -1 * rightMotorSetting > maxMotorSpeed) {
                report("ERROR: logic error in OOB controller!");
            }

            report("turning- l: " + leftMotorSetting + " r: " + rightMotorSetting);

            return;
        }

        public static double normalizeOrientation(double orientation) {
            while (orientation > Math.PI) {
                orientation -= 2*Math.PI;
            }

            while (orientation <= -1*Math.PI) {
                orientation +=  2*Math.PI;
            }

            return orientation;
        }
    }
}